#ifndef ________MOTOR_FOC_H________
#define ________MOTOR_FOC_H________

#include "motor_config.h"
#include <stdint.h>
#include "motor_adc.h"
#include "motor_pwm.h"
#include "user_config.h"

#ifdef __cplusplus
extern "C" {
#endif 

typedef enum foc_err_t{
	FOC_Err_None						=	0,
	FOC_Err_Motor_OverCurrent			=	1,
	FOC_Err_Motor_OverVoltage			=	2,
	FOC_Err_Motor_UnderVoltage			=	3,
	FOC_Err_Motor_OverTemperature		=	4,
	FOC_Err_Motor_Hall_Error			=	5,
	FOC_Err_Motor_Curr_Sensor_Failed	=	6,
	FOC_Err_Motor_Start_Failed			=	7, // 新增启动失败错误
	FOC_Err_Motor_Param_Invalid 		=	8,  // 电机参数无效
	FOC_Err_Motor_Hall_Calib_Error		= 	9,  // 霍尔校准错误
	FOC_Err_Motor_Runtime_Error			= 	10,  // 电机运行错误
	FOC_Err_MAX_ERRORS,
}FOC_Error_Type_e;

typedef enum{
	SOFT_START_NOT_STARTED	=	0,
	SOFT_START_IN_PROGRESS	=	1,
	SOFT_START_COOLDOWN		=	2,
	SOFT_START_SUCCESS		=	3,
	SOFT_START_FAILED		=	4
}Motor_SoftStart_Type_e;

typedef struct foc_param_t{
	uint16_t z_ctrlTypSel;      // 控制类型选择
    uint16_t b_fieldWeakEna;    // 弱磁使能
    uint16_t id_fieldWeakMax;   // 最大弱磁电流
    uint16_t a_phaAdvMax;       // 最大相位提前角
    uint16_t r_fieldWeakHi;     // 弱磁高阈值
    uint16_t r_fieldWeakLo;     // 弱磁低阈值
    uint16_t i_max;             // 最大电流限制
    uint16_t n_max;             // 最大转速限制
    uint16_t b_angleMeasEna;    // 角度测量使能
    uint16_t z_selPhaCurMeasABC;// 相电流测量选择
    uint16_t b_diagEna;         // 诊断使能
	uint16_t bat_overvoltage_threshold;		// 电池过压阈值
    uint16_t bat_undervoltage_threshold;	// 电池欠压阈值
    uint16_t temp_overtemp_threshold;		// 温度过温阈值
    uint16_t b_autoRecoveryEna;    // 自动恢复使能
    uint16_t autoRecoveryDelay;    // 自动恢复延迟时间(ms)
}FOC_Param_Type;

// FOC 输入结构体
typedef struct {
    uint8_t b_motEna;           // 电机使能
    uint8_t z_ctrlModReq;       // 控制模式请求
    int16_t r_inpTgt;           // 输入目标
    uint8_t b_hallA;            // 霍尔A
    uint8_t b_hallB;            // 霍尔B
    uint8_t b_hallC;            // 霍尔C
    int16_t i_phaAB;            // AB相电流
    int16_t i_phaBC;            // BC相电流
	int16_t i_phaAC;			// AC相电流
	int16_t t_motA;				// A相绕组温度
	int16_t t_motB;				// B相绕组温度
	int16_t t_motC;				// C相绕组温度
	int16_t t_drv;				// 驱动模块温度
    int16_t i_DCLink;           // DC链路电流
    int16_t a_mechAngle;        // 机械角度
} FOCInputs;

// FOC 输出结构体
typedef struct {
    int16_t DC_phaA;            // A相PWM占空比
    int16_t DC_phaB;            // B相PWM占空比
    int16_t DC_phaC;            // C相PWM占空比
    int16_t n_mot;              // 电机转速
    int16_t a_elecAngle;        // 电角度
    uint8_t z_errCode;          // 错误代码
} FOCOutputs;

// PI控制器结构
typedef struct {
    int32_t kp;         // 比例增益
    int32_t ki;         // 积分增益
    int32_t integral;  // 积分项
    int32_t max_output; // 最大输出限制
    int32_t min_output; // 最小输出限制
} PIController;

typedef struct {
    int32_t filter_alpha;      // 滤波系数（Q12格式）
    uint32_t min_sample_time;  // 最小采样时间（微秒）
    int16_t max_angle_diff;    // 最大角度差阈值
    int16_t poles;             // 电机极对数
} SpeedEstimateParamType;

// 电机参数结构体
typedef struct {
    float phase_resistance;    // 相电阻（欧姆）
    float phase_inductance;    // 相电感（亨利）
    float back_emf_constant;   // 反电动势常数（V/rad/s）
    int16_t poles;             // 极对数
    int16_t max_current;       // 最大电流（A）
    int16_t max_voltage;       // 最大电压（V）
    int16_t max_rpm;           // 最大转速（RPM）
} MotorParamType;

// 霍尔传感器校准参数
typedef struct {
    int16_t hall_offset[8];    // 霍尔状态到角度的偏移量
    int16_t hall_phase_error;  // 霍尔安装相位误差
    uint8_t hall_order[6];     // 霍尔信号顺序
} HallCalibParamType;

// 电流校准参数结构体
typedef struct {
    int16_t dc_link_offset;     // DC链路电流偏移量
    float dc_link_scale;        // DC链路电流比例因子
    uint8_t is_calibrated;      // 校准状态标志
}CurrentCalibParam;

typedef struct foc_t{
	FOCInputs inputs;
	FOCOutputs outputs;
	FOC_Param_Type param;
	int32_t internalStates[10]; // 内部状态变量

	// 缓启动相关变量
    Motor_SoftStart_Type_e softStartStatus;    // 缓启动状态
    uint8_t softStartAttempts;  // 当前尝试次数
    uint32_t softStartTime;     // 启动开始时间（毫秒）
    
    // 运行时检测相关变量
    uint8_t hallErrorCount;     // 霍尔错误计数
    uint8_t currentErrorCount;  // 电流错误计数
    uint8_t lastHallState;      // 上一次霍尔状态

     // PI控制器
    PIController id_controller;  // d轴电流PI控制器
    PIController iq_controller;  // q轴电流PI控制器
    PIController speed_controller; // 速度环PI控制器

    // 速度滤波
    int32_t speed_filtered;     // 滤波后的速度
    int32_t speed_filter_coef;   // 滤波系数
	// 速度估算优化相关变量
    int32_t speed_filter_alpha;      // 滤波系数（0-1之间，Q12格式）
    uint32_t last_update_time;       // 上一次速度更新时间
    int16_t prev_elec_angle;         // 上一次电角度
    int32_t angle_accumulator;       // 角度累积器（用于高精度计算）
    int32_t angle_accumulator_prev;  // 上一次角度累积器值
	SpeedEstimateParamType speed_params;
	int32_t speed_accumulator;     // 速度累积器（用于平均滤波）

	MotorParamType motor_param;
	HallCalibParamType hall_params;
	CurrentCalibParam current_calib;  // 电流校准参数
	FOC_Error_Type_e error_code;          // 当前错误代码
    uint16_t error_counters[FOC_Err_MAX_ERRORS];  // 各种错误的计数器

	uint32_t faultStartTime;       // 故障发生时间
    uint8_t autoRecoveryAttempt;   // 自动恢复尝试次数
}FOC_Type;

typedef struct {
    FOC_Param_Type param;
    MotorParamType motor_param;
    HallCalibParamType hall_params;
	CurrentCalibParam current_calib;
} FOC_Config_Type;

typedef struct {
    int16_t speed;
    int16_t current;
    int16_t voltage;
    int16_t temperature;
    uint8_t error_code;
} FOC_Telemetry_Type;

typedef struct {
    uint8_t enable;
    uint8_t mode;
    int16_t target;
} FOC_Command_Type;

// FOC事件日志记录函数
typedef enum {
    FOC_LOG_EVENT_INIT = 0,
    FOC_LOG_EVENT_START,
    FOC_LOG_EVENT_STOP,
    FOC_LOG_EVENT_FAULT,
    FOC_LOG_EVENT_TEMPERATURE_WARNING,
    FOC_LOG_EVENT_CURRENT_WARNING,
    FOC_LOG_EVENT_SPEED_CHANGE,
    FOC_LOG_EVENT_MODE_CHANGE
} FOC_LogEventType_e;

// 简单的日志缓冲区结构
#define FOC_LOG_BUFFER_SIZE 32
typedef struct {
    uint32_t timestamp;
    FOC_LogEventType_e event_type;
    uint16_t data;
} FOC_LogEntry_t;


// 数学常量
#define PI 3.14159265358979323846f
#define SQRT3 1.73205080756887729353f
#define SQRT3_2 0.86602540378443864676f
#define ONE_BY_SQRT3 0.57735026918962576451f

// 固定点数学运算
#define FIXED_SHIFT 12
#define FIXED_SCALE (1 << FIXED_SHIFT)

#if PWM_Duty_Max != 1000
#error "PWM_Duty_Max should be 1000 for correct operation"
#endif

#if FIXED_SHIFT != 12
#error "FIXED_SHIFT should be 12 for correct fixed-point arithmetic"
#endif

// 将浮点数转换为定点数
static inline int32_t float_to_fixed(float value) {
    return (int32_t)(value * FIXED_SCALE);
}

// 将定点数转换为浮点数
static inline float fixed_to_float(int32_t value) {
    return (float)value / FIXED_SCALE;
}

extern const MotorParamType DEFAULT_MOTOR_PARAMS;

void FOC_Init(FOC_Type* controller);
void FOC_Step(FOC_Type* controller, int16_t bat_voltage, int16_t temperature);
void FOC_SetParameters(FOC_Type* controller, const FOC_Param_Type* params);
void FOC_SetInputs(FOC_Type* controller, const FOCInputs* inputs);
void FOC_GetOutputs(FOC_Type* controller, FOCOutputs* outputs);
void FOC_ClarkeParkTransform(int16_t i_alpha, int16_t i_beta, int16_t angle, int16_t* i_d, int16_t* i_q);
void FOC_InverseClarkeParkTransform(int16_t u_d, int16_t u_q, int16_t angle, int16_t* u_alpha, int16_t* u_beta);
void FOC_SpaceVectorModulation(int16_t u_alpha, int16_t u_beta, int16_t* dutyA, int16_t* dutyB, int16_t* dutyC);
uint8_t FOC_SoftStart(FOC_Type* controller);
uint8_t FOC_RuntimeCheck(FOC_Type* controller);

void FOC_UpdateInputsFromADC(FOC_Type* controller);
void FOC_LogEvent(FOC_LogEventType_e event_type, uint16_t data);
uint8_t FOC_GetLogEntry(uint16_t index, FOC_LogEntry_t* entry);
void FOC_ClearLogBuffer(void);
void FOC_Reset(FOC_Type* controller);
void FOC_AutoRecovery(FOC_Type* controller);
static uint8_t FOC_ValidateADCData(FOC_Type* controller);
uint8_t FOC_SelfTest(FOC_Type* controller);
void FOC_Data_Transmit(FOC_Type* controller);
void FOC_Data_Parse(FOC_Type* controller, uint8_t* data, uint16_t length);
void FOC_SaveConfig(FOC_Type* controller);
void FOC_LoadConfig(FOC_Type* controller);
uint8_t FOC_CalibrateCurrent(FOC_Type* controller);
void FOC_SetCurrentCalibration(FOC_Type* controller, int16_t offset, float scale);
int16_t FOC_GetCalibratedCurrent(FOC_Type* controller, int16_t raw_current);

void FOC_ClearFaults(FOC_Type* controller);
static void FOC_SetError(FOC_Type* controller, FOC_Error_Type_e error_code);

#ifdef __cplusplus
}
#endif

#endif // !________MOTOR_FOC_H________
